package it.unicam.cs.cosy.bioshape.core;

import java.io.IOException;
import java.util.HashMap;
import java.util.Iterator;
import java.util.Map;

import javax.vecmath.Matrix3d;
import javax.vecmath.Matrix4d;
import javax.vecmath.Vector3d;

import vclip.ClosestFeaturesHT;
import vclip.ConvexPolyhedron;
import vclip.DistanceReport;
import vclip.PolyTree;

public class TestVClipLuca {
    public static void main(String[] args) {
        // Map<String, PolyTree> library = new HashMap<String, PolyTree>();
        // try {
        // PolyTree.scanLibrary(
        // "simulations/simulationSC/cylinder-sphere-vclip-library.txt",
        // library, true);
        // } catch (IOException e) {
        // System.out.println("Eccezione di IO " + e);
        // }
        // System.out.println(library);
        // PolyTree concaveSphere = library.get("concavesphere");
        // if (concaveSphere == null) {
        // System.out.println("null!");
        // return;
        // }
        // ConvexPolyhedron convexPol = concaveSphere.getPolyhedron();
        // System.out.println("Convex Polyhedron\n" + convexPol);
        // Vector3d centerOfMass = new Vector3d();
        // concaveSphere.centerOfMass(centerOfMass);
        // System.out.println("Center of mass\n" + centerOfMass);
        // Matrix4d transformation = concaveSphere.getTransform();
        // System.out.println("Transformation\n" + transformation);
        // boolean atomic = concaveSphere.isAtomic();
        // System.out.println("Is atomic? " + atomic);
        // System.out.println("Radius = " + concaveSphere.radius());
        // System.out.println("Name = " + concaveSphere.getName());
        // System.out.println("Number of components " +
        // concaveSphere.numComponents());
        // System.out.println("Number of leaves " + concaveSphere.numLeaves());
        // System.out.println("Number of nodes " + concaveSphere.numNodes());
        // Iterator<PolyTree> components = concaveSphere.getComponents();
        // while(components.hasNext()) {
        // PolyTree p = components.next();
        // System.out.println("Component " + p.getName());
        // System.out.println(p);
        // System.out.println("Component " + p.getName() + "'s transformation");
        // System.out.println(p.getTransform());
        // }
        // // Traslo
        // Matrix4d trasl = new Matrix4d(new double[]
        // {1,0,0,10,0,1,0,20,0,0,1,30,0,0,0,1});
        // concaveSphere.setTransform(trasl);
        // transformation = concaveSphere.getTransform();
        // System.out.println("Transformation\n" + transformation);
        // atomic = concaveSphere.isAtomic();
        // System.out.println("Is atomic? " + atomic);
        // System.out.println("Name = " + concaveSphere.getName());
        // concaveSphere.centerOfMass(centerOfMass);
        // System.out.println("Center of mass\n" + centerOfMass);
        // convexPol = concaveSphere.getPolyhedron();
        // System.out.println("Convex Polyhedron\n" + convexPol);
        // System.out.println("Radius = " + concaveSphere.radius());
        // System.out.println("Number of components " +
        // concaveSphere.numComponents());
        // System.out.println("Number of leaves " + concaveSphere.numLeaves());
        // System.out.println("Number of nodes " + concaveSphere.numNodes());
        // components = concaveSphere.getComponents();
        // while(components.hasNext()) {
        // PolyTree p = components.next();
        // System.out.println("Component " + p.getName());
        // System.out.println(p);
        // System.out.println("Component " + p.getName() + "'s transformation");
        // System.out.println(p.getTransform());
        // }
        double[] matrixValues = { -0.7356722380624665, 0.6578400161958071,
                -0.16134705214432118, 19.999999999999996, -0.3145713389561378,
                -0.5427881417085569, -0.7787335269062929, 65.00002001039411,
                0.5998593425558238, 0.5221374783637656, -0.606251781718144,
                20.00000000000003, 0.0, 0.0, 0.0, 1.0 };
        Matrix4d matrix2 = new Matrix4d(matrixValues);
        // Calcolo l'inversa
        Matrix4d inverseMatrix2 = new Matrix4d();
        inverseMatrix2.invert(matrix2);
        System.out.println("Matrice");
        System.out.println(matrix2);
        System.out.println("Matrice inversa calcolata");
        System.out.println(inverseMatrix2);
        // Calcolo trasposta
        Matrix4d transposeMatrix2 = new Matrix4d();
        transposeMatrix2.transpose(matrix2);
        System.out.println("Matrice trasposta");
        System.out.println(transposeMatrix2);
        // Calcolo inversa con trasposta
        Matrix3d matrix2TransposeRot = new Matrix3d();
        matrix2.get(matrix2TransposeRot);
        matrix2TransposeRot.transpose();
        Matrix4d inverseWithTransposeMatrix2 = new Matrix4d();
        inverseWithTransposeMatrix2.setRotationScale(matrix2TransposeRot);
        inverseWithTransposeMatrix2.m33 = 1;
        Vector3d translation2 = new Vector3d();
        matrix2.get(translation2);
        matrix2TransposeRot.negate();
        matrix2TransposeRot.transform(translation2);
        inverseWithTransposeMatrix2.setTranslation(translation2);
        System.out.println("Matrice inversa con trasposta");
        System.out.println(inverseWithTransposeMatrix2);
        Matrix4d inverse2 = new Matrix4d();
        inverseWithTranspose(matrix2, inverse2);
        System.out
                .println("Matrice inversa con trasposta calcolata dalla funzione");
        System.out.println(inverse2);
        double[] matrix1Values = { 0.541675220419702, 0.541675220419702,
                -0.642787609686539, 32.54, 0.113098303243884,
                0.710770780704124, 0.694272044014884, 57.81, 0.832944613636838,
                -0.448768150474739, 0.323744370967065, -31.009, 0, 0, 0, 1 };
        Matrix4d matrix1 = new Matrix4d(matrix1Values);
        Matrix4d inverseMatrix1 = new Matrix4d();
        inverseMatrix1.invert(matrix1);
        System.out.println("Matrice 1");
        System.out.println(matrix1);
        System.out.println("Matrice 1 inversa calcolata");
        System.out.println(inverseMatrix1);
        Matrix4d inverse1 = new Matrix4d();
        inverseWithTranspose(matrix1, inverse1);
        System.out
                .println("Matrice 1 inversa con trasposta calcolata dalla funzione");
        System.out.println(inverse1);
        // Calcolo matrice1 x matrice2^-1 - primo modo
        Matrix4d multiplication = new Matrix4d();
        multiplication.mul(matrix1, inverse2);
        System.out.println("matrice1 x matrice2^-1");
        System.out.println(multiplication);
        // Calcolo matrice1 x matrice2^-1 - secondo modo
        // Copia di matrix1
        Matrix4d matrix1Copy = new Matrix4d(matrix1);
        matrix1Copy.mul(inverse2);
        System.out.println("matrice1 x matrice2^-1 - secondo modo");
        System.out.println(matrix1Copy);
        Matrix4d multiplicationWithFunction = new Matrix4d();
        fromReferenceFrame2ToReferenceFrame1(matrix1, matrix2,
                multiplicationWithFunction);
        System.out.println("matrice1 x matrice2^-1 with function");
        System.out.println(multiplicationWithFunction);

        PolyTree box = PolyTree.createBox("myBox", 1, 1, 1);
        System.out.println("myBox Matrix " + box.getTransform());
        System.out.println("myBox Polyhedron " + box.getPolyhedron());

        PolyTree cone = PolyTree.createCone("myCone", 2, 4, 3, 3);
        System.out.println("myCone Matrix " + cone.getTransform());
        System.out.println("myCone Polyhedron " + cone.getPolyhedron());

        Matrix4d m1 = new Matrix4d();
        Matrix4d m2 = new Matrix4d();
        Matrix4d X2r1r = new Matrix4d();
        Matrix3d rot = new Matrix3d();
        double x;
        for (x = 10; x > 0; x--) {
            rot.rotZ(Math.toRadians(45));
            m1.setIdentity();
            m1.set(rot);
            m1.setTranslation(new Vector3d(3, 0, 0));
            // System.out.println("Matrix m1");
            // System.out.println(m1);
            rot.rotY(Math.toRadians(0));
            m2.setIdentity();
            m2.set(rot);
            m2.setTranslation(new Vector3d(x, 0, 0));
            // System.out.println("Matrix m2");
            // System.out.println(m2);
            Matrix4d inv1 = new Matrix4d();
            inverseWithTranspose(m1, inv1);
            X2r1r.mul(inv1, m2);
            // System.out.println("Matrix X2r1r");
            // System.out.println(X2r1r);
            DistanceReport drep = new DistanceReport();
            double distance = box.vclip(drep, cone, X2r1r, -1, null);
            System.out.println("x = " + x + " Distance = " + distance);
            System.out.println("Distance Report, Closest Distance: "
                    + drep.getClosestDistance());
            System.out
                    .println("DR, max close pairs " + drep.getMaxClosePairs());
            System.out.println("Dr, max pair distance "
                    + drep.getMaxPairDistance());
            System.out.println("DR, promotion tolerance "
                    + drep.getPromotionTolerance());
            System.out.println("DR, num close pairs " + drep.numClosePairs());
            System.out.println("DR, promotion enabled "
                    + drep.promotionEnabled());
            System.out.println("DR, Closest Pair " + drep.getClosestPair());
            System.out.println("CP, dist " + drep.getClosestPair().dist);
            System.out.println("CP, feat1 "
                    + drep.getClosestPair().feat1.getName());
            System.out.println("CP, feat2 "
                    + drep.getClosestPair().feat2.getName());
            System.out.println("CP, pnt1 " + drep.getClosestPair().pnt1);
            System.out.println("CP, pnt2 " + drep.getClosestPair().pnt2);
            System.out.println("CP, nrml " + drep.getClosestPair().nrml);
            System.out.println("M1 " + m1);
            Vector3d pippo = new Vector3d(drep.getClosestPair().nrml);
            m1.transform(pippo);
            System.out.println("CP, nrml x X1lr = " + pippo);
            drep.transformFirstPoints(m1);
            System.out.println("CP, nrml dopo transform first points m1 = "
                    + drep.getClosestPair().nrml);
            System.out.println("CP, pnt1 dopo transform first points m1 ="
                    + drep.getClosestPair().pnt1);
            System.out.println("CP, pnt2 dopo transform first points m1 ="
                    + drep.getClosestPair().pnt2);
            drep.transformSecondPoints(m2);
            System.out.println("CP, nrml dopo transform first points m2 = "
                    + drep.getClosestPair().nrml);
            System.out.println("CP, pnt1 dopo transform first points m2 ="
                    + drep.getClosestPair().pnt1);
            System.out.println("CP, pnt2 dopo transform first points m2 ="
                    + drep.getClosestPair().pnt2);

            System.out.println("\n\nClosest pairS");
            for (int i = 0; i < drep.getClosePairs().length; i++) {
                System.out.println("CP " + i + ", dist "
                        + drep.getClosePairs()[i].dist);
                System.out.println("CP " + i + ", feat1 "
                        + drep.getClosePairs()[i].feat1.getName());
                System.out.println("CP " + i + ", feat2 "
                        + drep.getClosePairs()[i].feat2.getName());
                System.out.println("CP " + i + ", nrml "
                        + drep.getClosePairs()[i].nrml);
                System.out.println("CP " + i + ", pnt1 "
                        + drep.getClosePairs()[i].pnt1);
                System.out.println("CP " + i + ", pnt2 "
                        + drep.getClosePairs()[i].pnt2);
            }
            if (distance < 0)
                break;
        }
        for (x = 10; x > 0; x--) {
            rot.rotX(Math.toRadians(45));
            m1.setIdentity();
            m1.set(rot);
            m1.setTranslation(new Vector3d(3, 0, 0));
            box.getTransform().set(m1);
            // System.out.println("Matrix of box");
            // System.out.println(box.getTransform());
            rot.rotY(Math.toRadians(30));
            m2.setIdentity();
            m2.set(rot);
            m2.setTranslation(new Vector3d(x, 0, 0));
            cone.getTransform().set(m2);
            // System.out.println("Matrix of cone");
            // System.out.println(cone.getTransform());
            X2r1r.setIdentity();
            // System.out.println("Matrix X2r1r");
            // System.out.println(X2r1r);
            DistanceReport drep = new DistanceReport();
            double distance = box.vclip(drep, cone, X2r1r, -1, null);
            System.out.println("x = " + x + " Distance = " + distance);
            if (distance < 0)
                break;
        }

        Map<String, PolyTree> library = new HashMap<String, PolyTree>();
        try {
            PolyTree.scanLibrary("misc/PolyTreeExamples.txt", library, true);
        } catch (IOException e) {
            System.out.println("Eccezione di IO " + e);
        }
        System.out.println(library);
        PolyTree twoCubes1 = library.get("two-cubes");

        PolyTree twoCubes2 = library.get("two-cubes");

        for (x = 10; x > 0; x--) {
            rot.rotX(Math.toRadians(45));
            m1.setIdentity();
            m1.set(rot);
            m1.setTranslation(new Vector3d(3, 0, 0));
            // System.out.println("Matrix m1");
            // System.out.println(m1);
            rot.rotY(Math.toRadians(30));
            m2.setIdentity();
            m2.set(rot);
            m2.setTranslation(new Vector3d(x, 0, 0));
            // System.out.println("Matrix m2");
            // System.out.println(m2);
            Matrix4d inv1 = new Matrix4d();
            inverseWithTranspose(m1, inv1);
            X2r1r.mul(inv1, m2);
            // System.out.println("Matrix X2r1r");
            // System.out.println(X2r1r);
            DistanceReport drep = new DistanceReport();
            double distance = twoCubes1.vclip(drep, twoCubes2, X2r1r, -1, null);
            System.out.println("x = " + x + " Distance = " + distance);
            if (distance < 0)
                break;
        }
        for (x = 10; x > 0; x = x - 1) {
            rot.rotX(Math.toRadians(45));
            m1.setIdentity();
            m1.set(rot);
            m1.setTranslation(new Vector3d(3, 0, 0));
            twoCubes1.getTransform().set(m1);
            twoCubes1.setTransform(m1);
            // System.out.println("Matrix of box");
            // System.out.println(box.getTransform());
            rot.rotY(Math.toRadians(30));
            m2.setIdentity();
            m2.set(rot);
            m2.setTranslation(new Vector3d(x, 0, 0));
            twoCubes2.getTransform().set(m2);
            twoCubes2.setTransform(m2);
            // System.out.println("Matrix of cone");
            // System.out.println(cone.getTransform());
            X2r1r.setIdentity();
            // System.out.println("Matrix X2r1r");
            // System.out.println(X2r1r);
            DistanceReport drep = new DistanceReport();
            double distance = twoCubes1.vclip(drep, twoCubes2, X2r1r, -1, null);
            System.out.println("x = " + x + " Distance = " + distance);
            if (distance < 0)
                break;
        }
        System.out.println("-----");
        for (x = 10; x > 0; x -= 1 / 25.0) {
            rot.rotX(Math.toRadians(45));
            m1.setIdentity();
            m1.set(rot);
            m1.setTranslation(new Vector3d(3, 0, 0));
            // System.out.println("Matrix m1");
            // System.out.println(m1);
            rot.rotY(Math.toRadians(30));
            m2.setIdentity();
            m2.set(rot);
            m2.setTranslation(new Vector3d(x, 0, 0));
            // System.out.println("Matrix m2");
            // System.out.println(m2);
            Matrix4d inv1 = new Matrix4d();
            inverseWithTranspose(m1, inv1);
            X2r1r.mul(inv1, m2);
            // System.out.println("Matrix X2r1r");
            // System.out.println(X2r1r);
            DistanceReport drep = new DistanceReport();
            double distance = box.vclip(drep, box, X2r1r, -1, null);
            System.out.println("x = " + x + " Distance = " + distance);
            if (distance < 0) {
                System.out.println("Distanca < 0 " + distance);
                break;
            }

        }
        ClosestFeaturesHT ht = new ClosestFeaturesHT();
        System.out.println("-----");
        for (x = 10; x > 0; x -= 1 / 25.0) {
            rot.rotX(Math.toRadians(45));
            m1.setIdentity();
            m1.set(rot);
            m1.setTranslation(new Vector3d(3, 0, 0));
            // System.out.println("Matrix m1");
            // System.out.println(m1);
            rot.rotY(Math.toRadians(30));
            m2.setIdentity();
            m2.set(rot);
            m2.setTranslation(new Vector3d(x, 0, 0));
            // System.out.println("Matrix m2");
            // System.out.println(m2);
            Matrix4d inv1 = new Matrix4d();
            inverseWithTranspose(m1, inv1);
            X2r1r.mul(inv1, m2);
            // System.out.println("Matrix X2r1r");
            // System.out.println(X2r1r);
            DistanceReport drep = new DistanceReport();
            double distance = twoCubes1.vclip(drep, twoCubes1, X2r1r, -1, ht);
            System.out.println("x = " + x + " Distance = " + distance);
            if (distance < 0) {
                System.out.println("Distance < 0 " + distance);
                break;
            }
        }
    }

    /**
     * Calculate the transformation matrix that transforms from a reference
     * frame 2 to a reference frame 1, both frames given as transformation
     * matrices as well.
     * 
     * @param referenceFrame1TransformationMatrix
     *            the transformation matrix transforming to reference frame 1,
     *            left unchanged
     * @param referenceFrame2TransformationMatrix
     *            the transformation matrix transforming to reference frame 2,
     *            left unchanged
     * @param r2r1TransformationMatrix
     *            the transformation matrix transforming from reference frame 2
     *            to reference frame 1
     */
    public static void fromReferenceFrame2ToReferenceFrame1(
            Matrix4d referenceFrame1TransformationMatrix,
            Matrix4d referenceFrame2TransformationMatrix,
            Matrix4d r2r1TransformationMatrix) {
        Matrix4d inverse2 = new Matrix4d();
        inverseWithTranspose(referenceFrame2TransformationMatrix, inverse2);
        r2r1TransformationMatrix.mul(referenceFrame1TransformationMatrix,
                inverse2);
    }

    /**
     * Calculate the inverse of a transformation matrix 4x4 (composed of a
     * rotational part rot, which is a 3x3 matrix, a translational part t, which
     * is a 3 component vector, and the last row equal to 0 0 0 1) using the
     * property that this is always equal to the 4x4 matrix composed by the
     * transpose of rot, and the transformation of t by the negation of the
     * transpose of rot: t' = -rot^T * t. The last row is again 0, 0, 0, 1
     * 
     * @param original
     *            the orginal matrix to invert, left unchanged
     * @param inversed
     *            the inverted matrix
     */
    public static void inverseWithTranspose(Matrix4d original, Matrix4d inversed) {
        Matrix3d originalTransposeRot = new Matrix3d();
        original.get(originalTransposeRot);
        originalTransposeRot.transpose();
        inversed.setRotationScale(originalTransposeRot);
        inversed.m30 = 0;
        inversed.m31 = 0;
        inversed.m32 = 0;
        inversed.m33 = 1;
        Vector3d translationOriginal = new Vector3d();
        original.get(translationOriginal);
        originalTransposeRot.negate();
        originalTransposeRot.transform(translationOriginal);
        inversed.setTranslation(translationOriginal);
    }

}
